/* Copyright (c) 2011 Alexandre Défossez
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */
 #include "ElasticModel.hpp"

ElasticModel::ElasticModel(){}

void ElasticModel::init(Point3d currentPos, Point3d dest, Vec3d Vi,
                 double tau){
  this->dest = dest;
  this->beginT = globalClock.GetElapsedTime();
  this->tau = tau;

  B[0] = currentPos.x - dest.x;
  B[1] = currentPos.y - dest.y;
  B[2] = currentPos.z - dest.z;

  A[0] = Vi[0] + B[0]/(2*tau);
  A[1] = Vi[1] + B[1]/(2*tau);
  A[2] = Vi[2] + B[2]/(2*tau);
}

Point3d ElasticModel::getPosition(){
  double t = globalClock.GetElapsedTime() - beginT;
  double x = exp(-t/(2.*tau))*(A[0]*t + B[0]) + dest.x;
  double y = exp(-t/(2.*tau))*(A[1]*t + B[1]) + dest.y;
  double z = exp(-t/(2.*tau))*(A[2]*t + B[2]) + dest.z;
  return Point3d(x,y,z);
}

Vec3d ElasticModel::getSpeed(){
  double t = globalClock.GetElapsedTime() - beginT;
  double vX = exp(-t/(2.*tau))*((-1./(2.*tau))*(A[0]*t + B[0])+A[0]);
  double vY = exp(-t/(2.*tau))*((-1./(2.*tau))*(A[1]*t + B[1])+A[1]);
  double vZ = exp(-t/(2.*tau))*((-1./(2.*tau))*(A[2]*t + B[2])+A[2]);
  return Vec3d(vX,vY,vZ);
}
